import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
    # 获取包的共享目录
    pkg_share = get_package_share_directory('xunxian')

    # 启动参数
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    urdf_file_name = 'my_car.urdf'
    world_file_name = 'paodao_2.world'

    urdf = os.path.join(pkg_share, 'urdf', urdf_file_name)
    world = os.path.join(pkg_share, 'worlds', world_file_name)

    # 启动Gazebo
    start_gazebo_cmd = ExecuteProcess(
        cmd=['gazebo', '--verbose', "-s", "libgazebo_ros_init.so", "-s", "libgazebo_ros_factory.so" ,world],
        output='screen'
    )

    # 生成机器人模型
    spawn_entity_cmd = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-entity', 'waking_robot12', '-file', urdf, '-x', '0', '-y', '0', '-z', '0'],
        output='screen'
    )

    # 加载URDF文件
    robot_state_publisher_cmd = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time}],
        arguments=[urdf]
    )

    return LaunchDescription([
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='true',
            description='Use simulation (Gazebo) clock if true'
        ),
        start_gazebo_cmd,
        spawn_entity_cmd,
        robot_state_publisher_cmd,
    ])
